/* Includes ----------------------------------------------------------------- */
#include <tim.h>

#include "../algorithm/filter.h"
#include "../algorithm/integration.h"
/* Exported constants ------------------------------------------------------- */
extern uint8_t encoder_overcount[2];
/* Exported macro ----------------------------------------------------------- */
/* Exported types ----------------------------------------------------------- */

typedef enum {
  FORE = 0,
  BACK,
  VERTICAL,
  KEEP,
} MoveDirection_t;

typedef struct {
  float pwm_f;
  float pwm_b;
  float pwm_v;
  float pwm_pit;
  float pwm_rol;
} PidPwm_t;

typedef struct {
  float out;
  float integral;
  float error_last;
} PidOut_t;

typedef struct {
  float k;
  float kp;
  float ki;
  float kd;
} Pid_t;

typedef struct {
  Pid_t pid;
  PidOut_t out;

} Pid_Param_t;

/* Exported functions prototypes -------------------------------------------- */

float PID_AnglerSpeedLoop(float anglespeed_want, float angle_speed,
                          Pid_Param_t *pid_p);
float PID_AngleLoop(float angle_want, float angle, Pid_Param_t *pid_p);
float PID_SpeedLoop(float speed_want, float speed_now, Pid_Param_t *pid_p);

PidPwm_t Pid_Control_Task1(Euler_t angel, MoveDirection_t direction,
                           float expect_angel);
PidPwm_t Pid_Control_Task3(float l, MoveDirection_t direction, float expect_l);

#ifdef __cplusplus
}
#endif
